/* Private includes ----------------------------------------------------------*/
#include "hfi.h"
#include "fliter.h"
#include "main.h"
/* Private define ------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
// HFI hfi;
// HFI_PID_DEF HFI_PID;
// int hfi_cnt;
// float hfi_inj_volt_amp;
// float iq_bpf;
// float hfi_integrator;
// /* Private function prototypes -----------------------------------------------*/

// void HFI_Process(float iq_in, float id_in, float *lfp_iq, float *lfp_id, HFI *hfi)
// {
//   float hfi_cos;
//   float hfi_sin;
//   float temp1;
//   float temp2;
//   // float iq_bpf;
//   float hfi_pid_error;
//   float hfi_pid_out;
//   if(hfi_cnt >= HFI_CNT_FLAG)
//   {
//     hfi_cnt = 0;
//   }

//   temp1             = hfi_cnt;
//   temp2             = HFI_CNT_FLAG;
//   hfi_cos           = arm_cos_f32((temp1 / temp2) * 2.0f * PI);
//   hfi_sin           = arm_sin_f32((temp1 / temp2) * 2.0f * PI);
//   hfi->hfi_inj_volt = hfi_cos * hfi_inj_volt_amp;
//   hfi_cnt++;

//   IIR_Butterworth(iq_in, lfp_iq, &Iq_IIR_LPF);
//   IIR_Butterworth(id_in, lfp_id, &Id_IIR_LPF);
//   IIR_Butterworth(iq_in, &iq_bpf, &Iq_IIR_BPF);

//   temp1 = hfi_sin * iq_bpf;
//   IIR_Butterworth(temp1, &hfi_pid_error, &err_IIR_LPF);

//   hfi_pid_out = HFI_PID.P * hfi_pid_error + HFI_PID.I_Sum;

//   hfi->hfi_theta  = hfi_integrator;
//   hfi->hfi_we     = hfi_pid_out;
//   hfi_integrator += 0.00005f * hfi_pid_out;
//   if(hfi_integrator < 0.0f)
//   {
//     hfi_integrator += 2.0f * PI;
//   }
//   else if(hfi_integrator > (2.0f * PI))
//   {
//     hfi_integrator -= 2.0f * PI;
//   }

//   HFI_PID.I_Sum += (HFI_PID.I * hfi_pid_error) * 0.00005f;
// }

// void HFI_Init(void)
// {
//   float Iq_IIR_LPF_Param[8] = {1.0f, 1.0f, 0.0f, 1.0f, -0.984414127416096906f, 0.0f, 0.007792936291951552f, 1.0f};
//   float Id_IIR_LPF_Param[8] = {1.0f, 1.0f, 0.0f, 1.0f, -0.984414127416096906f, 0.0f, 0.007792936291951552f, 1.0f};
//   float Iq_IIR_BPF_Param[8] = {1.0f, 0.0f, -1.0f, 1.0f, -1.84506846156821180f, 0.93906250581749250994079f, 0.0304687470912537138f, 1.0f};
//   IIR_Butterworth_Coefficient_Init(Iq_IIR_LPF_Param, &Iq_IIR_LPF);
//   IIR_Butterworth_Coefficient_Init(Id_IIR_LPF_Param, &Id_IIR_LPF);
//   IIR_Butterworth_Coefficient_Init(Iq_IIR_BPF_Param, &Iq_IIR_BPF);
//   IIR_Butterworth_Coefficient_Init(Iq_IIR_LPF_Param, &err_IIR_LPF);

//   hfi_cnt          = 0;
//   hfi_inj_volt_amp = 2.4f;

//   HFI_PID.P      = 50.0f;
//   HFI_PID.I      = 500.0f;
//   HFI_PID.I_Sum  = 0.0f;
//   hfi_integrator = 0.0f;
//   hfi.hfi_theta  = 0.0f;
// }
